#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <experimental/filesystem>
#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

namespace fs = std::experimental::filesystem;
using namespace std;
using namespace cv;
// using namespace message_filters;

double timestamp_;
std::string path_save;
string img_topic;
string lidar_topic;

int count_ = 0;
void savePointCloudAsBin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, const std::string& filename);
void callback(const sensor_msgs::ImageConstPtr &front_left_msg,
                const sensor_msgs::PointCloud2ConstPtr &pc_left_msg
                );

int main(int argc, char **argv)
{
  path_save = argv[1]; //= "/cv/DataSet/MVS/all_record_raw/q23_seq0_7img2lidar/img7lidar2";
  img_topic = argv[2];//"/senyun/front_mid";
  lidar_topic = argv[3]; // /lidar/left
  ros::init(argc, argv, "synchronizer");
  ros::NodeHandle nh_;
  message_filters::Subscriber<sensor_msgs::Image> sub_1_;
  message_filters::Subscriber<sensor_msgs::PointCloud2> sub_8_;
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
  typedef message_filters::Synchronizer<MySyncPolicy> Sync;
  boost::shared_ptr<Sync> sync_;
  int count_;

  // syncNode synchronizer;

  count_ = 0;
  sub_1_.subscribe(nh_, "/camera/"+img_topic, 1);
  sub_8_.subscribe(nh_, lidar_topic, 1);
  sync_.reset(new Sync(MySyncPolicy(10), sub_1_,sub_8_));
  sync_->registerCallback(
    boost::bind(&callback, _1, _2)
    );
  ros::spin();
}

void savePointCloudAsBin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, const std::string& filename)
{
    // Open binary file for writing
    std::ofstream outfile(filename.c_str(), std::ios::out | std::ios::binary);
    // Write point cloud data to binary file
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        outfile.write((const char*) &cloud->points[i].x, sizeof(float));
        outfile.write((const char*) &cloud->points[i].y, sizeof(float));
        outfile.write((const char*) &cloud->points[i].z, sizeof(float));
        outfile.write((const char*) &cloud->points[i].intensity, sizeof(float));
    }
    // Close binary file
    outfile.close();
}

void callback(const sensor_msgs::ImageConstPtr &front_mid_msg,const sensor_msgs::PointCloud2ConstPtr &pc_left_msg)
  {
    if ((count_++ % 8) != 0)
    {
      return;
    }
    ROS_INFO("Synchronization successful");
    timestamp_ = ros::Time::now().toSec();
    string new_path_save = path_save +"/"+to_string(timestamp_);
    cv::Mat image = cv::Mat::zeros(front_mid_msg->height, front_mid_msg->width, CV_8UC3);
    memcpy(image.data, front_mid_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
    // cv::imshow("mid", image);
    // cv::waitKey(1);
    cv::imwrite(new_path_save + "_"+img_topic+".jpg", image);

    string lidar_save_path = new_path_save+"_lidar.bin";
    float data[4*400000];
    int max_num = 400000;
    int num=0;

    std::ofstream outfile(lidar_save_path, std::ios::binary);
    if (outfile.is_open())
    {
      for(int i=0;i<pc_left_msg->row_step * pc_left_msg->height ;i=i+pc_left_msg->point_step )
      {
          float x=static_cast<float>(*(reinterpret_cast<float const *>(&(pc_left_msg->data [i])+pc_left_msg->fields[0].offset )));
          float y=static_cast<float>(*(reinterpret_cast<float const *>(&(pc_left_msg->data [i])+pc_left_msg->fields[1].offset )));
          float z=static_cast<float>(*(reinterpret_cast<float const *>(&(pc_left_msg->data [i])+pc_left_msg->fields[2].offset )));
          float s=static_cast<float>(*(reinterpret_cast<float const *>(&(pc_left_msg->data [i])+pc_left_msg->fields[3].offset )));
          if (x==0 && y==0 && z==0) continue;
          outfile.write((const char*) &x, sizeof(float));
          outfile.write((const char*) &y, sizeof(float));
          outfile.write((const char*) &z, sizeof(float));
          outfile.write((const char*) &s, sizeof(float));
          // data[num*4+0]=z;
          // data[num*4+1]=y;
          // data[num*4+2]=x;
          // data[num*4+3]=intisity;
          num++;
          // printf("xyzi: %f %f %f %f \n",x,y,z,s);
          if(num>max_num){printf("point_cloud_data out of range re define the number of pointcloud\n");}
      }
    }

    outfile.close();

    cout << "save_path: " << new_path_save + "lidar.bin " <<num << endl;
    // exit(0);

  }
